#include "main.h"
#include "can.h"
#include "gpio.h"
#include "CAN_receive.h"
#include "detect_task.h"
#include "bsp_buzzer.h"

fromGimbalSTM_t gimbalSTM ;//上云台数据的结构体

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

uint16_t psc = 100;
uint16_t pwm = 200;

//定义电机信息读取。实际是宏
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->current_or_torque = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate_or_null = (data)[6];                                   \
    }

//低层次的工作数据
static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];
static motor_measure_t      motor_chassis[LENGTH_OF_MOTOR_ID];

//回调函数。用于can的信息获取。在fifo结构内。
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
    if(hcan==&CHASSIS_UPGIMBAL_CAN)
    {
      switch (rx_header.StdId)
      {

          #ifdef GM6020_CURRENTMODE
          case CAN_CHASSIS_LF_G:
          case CAN_CHASSIS_RF_G:
          case CAN_CHASSIS_RB_G:
          case CAN_CHASSIS_LB_G:
          {
                static uint8_t i = 0;
                //get motor id
              //根据我们实际使用的修改
              i = rx_header.StdId - CAN_CHASSIS_LF_M+CHASSIS_LF_M;           
                get_motor_measure(&motor_chassis[i], rx_data);
                detect_hook(i+CHASSIS_LF_M_TOE);
                break;
          }
          #endif

          case CAN_FROM_GIMBAL_XYW:
          {
            gimbalSTM.x=(int8_t)rx_data[4];
            gimbalSTM.y=(int8_t)rx_data[5];
            gimbalSTM.w=(int8_t)rx_data[6];
            gimbalSTM.nextmode=(int8_t)rx_data[7];
            detect_hook(GIMBALSTM32_TOE);
          }
          break;     

          case CAN_YAW_DOWN:
          {
            get_motor_measure(&motor_chassis[YAW_DOWN], rx_data);
            detect_hook(YAW_DOWN+CHASSIS_LF_M_TOE);
          }

          default:
          {
              break;
          }
      }
    }else if(hcan==&CHASSIS_CAN)
    {
      switch (rx_header.StdId)
        { 
             #ifdef GM6020_VOLTMODE
             case CAN_CHASSIS_LF_G:
             case CAN_CHASSIS_RF_G: 
             case CAN_CHASSIS_RB_G: 
             case CAN_CHASSIS_LB_G: 
              {
                  static uint8_t i = 0;
                  //get motor id
                  //根据我们实际使用的修改
                  i = rx_header.StdId - CAN_CHASSIS_LF_M+CHASSIS_LF_M;           
                  get_motor_measure(&motor_chassis[i], rx_data);
                  detect_hook(i+CHASSIS_LF_M_TOE);
                  break;
              }
             #endif

            case CAN_CHASSIS_LF_M:
            case CAN_CHASSIS_RF_M:
            case CAN_CHASSIS_RB_M:
            case CAN_CHASSIS_LB_M: 
            {
                static uint8_t i = 0;
                i = rx_header.StdId - CAN_CHASSIS_LF_M+CHASSIS_LF_M;           
                get_motor_measure(&motor_chassis[i], rx_data);
                detect_hook(i+CHASSIS_LF_M_TOE);
                break;
          }

          
          default:
          {
              break;
          }
        }
    }
}

/**
  * @brief          发送四个底盘电机控制电流[0,1,2,3]
  * @param[in]      motor1: (0) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor2: (1) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor3: (2) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor4: (3) 3508电机控制电流, 范围 [-16384,16384]MAX=20A
  * @retval         none
  */
void CAN_cmd_chassis_M3508(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_M_CAN2_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
  * @brief          发送四个底盘电机控制电流[4,5,6,7]
  * @param[in]      motor1: (4) 6020电机控制电压, 范围 [-30000,30000]
  * @param[in]      motor2: (5) 6020电机控制电压, 范围 [-30000,30000]
  * @param[in]      motor3: (6) 6020电机控制电压, 范围 [-30000,30000]
  * @param[in]      motor4: (7) 6020电机控制电压, 范围 [-30000,30000]
  * @retval         none
  */

void CAN_cmd_chassis_GM6020(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_G_CAN1_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;

    #ifdef GM6020_CURRENTMODE
    HAL_CAN_AddTxMessage(&CHASSIS_UPGIMBAL_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
    #endif
    #ifdef GM6020_VOLTMODE
    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
    #endif
}
/**
  * @brief          返回底盘电机 3508电机数据指针
  * @param[in]      i: 电机编号,范围[0,8]
  * @retval         电机数据指针
  */
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
    return &motor_chassis[i];
}

/**
  * @brief          返回上云台发送的数据结构体指针
  * @retval         上云台发送的数据结构体指针
  */
const fromGimbalSTM_t *get_gimbal_stm_point(void)
{
    return &gimbalSTM;
}

